#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>
#include "communication.h"
#include "control.h"

extern errno;

/***************************************
 * 函数功能: 连接至网络服务器
 * 参数: IP 服务器IP地址； port 服务器端口
 * 返回值: 网络对应的文件描述符 
 * 其他：连接不成功直接退出程序
 * *************************************/
int connectToServer(char * IP,char * port)
{
    int sockid;
    struct sockaddr_in servadd;

    if((sockid = socket(AF_INET,SOCK_STREAM,0)) == -1)
        oops("socket",1);
    
    bzero(&servadd,sizeof(servadd));

    servadd.sin_port = htons(atoi(port));
    servadd.sin_family = AF_INET;
    servadd.sin_addr.s_addr = inet_addr(IP);

    if(connect(sockid,(struct sockaddr *)&servadd,sizeof(servadd)) != 0)
        oops("connect",1);
    
    printf("connect succeded\r\n");
    return sockid;
}


/***************************************
 * 函数功能: 建立数据包
 * 参数: buffer 要建立的数据包地址； socket_port 服务器端口；Cmd 命令；argv 参数数组；argc 参数个数
 * 返回值: 无 
 * *************************************/
void buildTM(char buffer[],uint32_t socket_port,uint32_t Cmd,uint32_t *argv,int argc)
{
	SGAGV_TM tm;
	
	tm.IDH = htonl(0);
	tm.IDL = htonl(socket_port);
	tm.type = htonl(1);				// SGAGV=1
	tm.aipd	= htonl(1);				// APID=1 常规遥测
	tm.pkgLen = htonl(LEN_PKG);			// 从B0开始计算256字节
	tm.timeH = htonl(0);
	tm.timeL = htonl(0);
	tm.reserve = htonl(0);
	//tm.gpsX = htonl(*(uint32_t *)(&gpsX));
	tm.cmdid = htonl(Cmd);
    if(argv)
    {
        for(int i = 0;i<argc;i++)
            tm.v[i] = htonl(argv[i]);
    }
	
	//-----------------------------

	memcpy(buffer, 		&tm, 	sizeof(tm));
}

/***************************************
 * 函数功能: 读取n个字节 用于网络读
 * 参数: fd 文件描述符；ptr 存储字节的缓冲区；n 字节个数
 * 返回值: 读取失败返回-1； 读取成功返回读取的字节数
 * *************************************/
ssize_t readn(int fd,void *ptr,size_t n)
{
    size_t nleft;
    ssize_t nread;
    char * string = (char * )ptr;
    nleft=n;

    while (nleft>0)
    {
        if ((nread = read(fd,string,nleft))<0)
        {
            if (nleft == n)
                return -1;
            else
                break;
        }    
        else if (nread ==0)
            break;

        nleft -= nread;
        string += nread;
    }

    return(n-nleft);
}

/***************************************
 * 函数功能: 写n个字节 用于网络写
 * 参数: fd 文件描述符；ptr 存储字节的缓冲区；n 字节个数
 * 返回值: 写失败返回-1； 写成功返回写的字节数
 * *************************************/
ssize_t writen(int fd,void *ptr,size_t n)
{
    size_t nleft;
    ssize_t nwrite;
    char * string = (char * )ptr;

    nleft=n;

    while (nleft>0)
    {
        if ((nwrite = write(fd,string,nleft))<0)
        {
            if (nleft == n)
                return -1;
            else
                break;
        }    
        else if (nwrite ==0)
            break;

        nleft -= nwrite;
        string += nwrite;
    }

    return(n-nleft);
}
/***************************************
 * 函数功能: 解析数据包
 * 参数: buffer 接收的数据包
 * 返回值: 返回数据包
 * *************************************/
SGAGV_TC parseCmd(char buffer[])
{
	uint8_t tmpBuffer[LEN_PKG];
	SGAGV_TC tc;
	
	memcpy(tmpBuffer, 		buffer, 	LEN_PKG);
	
	for(int i=0;i<64;i++)
	{
		uint32_t tmp = ntohl(*(uint32_t *)(buffer+i*4));
		*(uint32_t *)(tmpBuffer+i*4) = tmp;
	}
	memcpy(&tc, tmpBuffer, 	sizeof(tc));
	
	//printf("TC time = %d\n",tc.timeH);
	//printf("TC cmdid = %d\n", tc.cmdid);
	return tc;
}

/***************************************
 * 函数功能: 发送命令
 * 参数: sockid 网络读写描述符；port 服务器的端口；cmd 要发送的命令；argv 要发送的参数；argc 要发送的参数个数
 * 返回值: 无
 * *************************************/
void sendCom(int sockid,uint32_t port,uint32_t cmd,uint32_t*argv,int argc)
{
    char send_buffer[LEN_PKG];
    buildTM(send_buffer,port,cmd,argv,argc);
    if(writen(sockid,send_buffer,LEN_PKG) < 0)
        oops("socket write error",1);
}

/***************************************
 * 函数功能: 从WEBOTS读命令
 * 参数: sockid 网络读写描述符
 * 返回值: 无
 * *************************************/
void readCom(int sockid)
{
    struct SGAGV_TC rc;
    char read_buffer[LEN_PKG];

    if(readn(sockid,read_buffer,LEN_PKG)<0)
            oops("socket read error",1);
        
    rc = parseCmd(read_buffer);

    
    recVarFromComm(&rc);
}

void oops(char *s,int num)
{
    perror(s);
    exit(num);
}